perm filename ARMSOL.SAI[PNT,HE] blob
sn#375118 filedate 1978-08-21 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00004 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 IFCR NOT DECLARATION($$PRGID) THENC
C00005 00003 INTERNAL INTEGER PROCEDURE ARMSOL(INTEGER ARM REAL ARRAY ANGLE
C00010 00004 ! Solve for last three joints
C00016 ENDMK
C⊗;
IFCR NOT DECLARATION($$PRGID) THENC
ENTRY;
BEGIN "ARMSOL" ENDC
COMMENT COPIED FROM [PNT,MSM] DEC 30, 1977;
DEFINE $ARMSOL=TRUE;
REQUIRE "HEADER.SAI" SOURCE_FILE;
PRELOAD_WITH -185.0,-175.0,6.5,-175.0,-101.0,-360.0,0.0, ! Yellow low stops;
! -45.0,-165.0,6.75,-395.0,-95.0,-110.0,-0.2; ! Blue low stops;
-45.0,-165.0,6.75,-295.0,-95.0,-110.0,-0.2; ! Blue low stops;
INTERNAL SAFE OWN REAL ARRAY LOSTOP[0:1,1:7];
PRELOAD_WITH 60.0,-5.0,27.5,140.0, 101.0, 360.0,3.89, ! Yellow high stops;
190.0,-50.0,33.0,205.0, 95.0, 200.0,3.80; ! Blue high stops;
INTERNAL SAFE OWN REAL ARRAY HISTOP[0:1,1:7];
PRELOAD_WITH -180.0,-90.0,14.0,-90.0,90.0,0.0, ! Yellow park position;
180.0,-90.0,14.0,-90.0,90.0,0.0; ! Blue park position;
INTERNAL SAFE OWN REAL ARRAY PARK[0:1,1:6];
PRELOAD_WITH 0.75,0.5,4.5,0.2,0.2,0.4,5.0, ! Yellow joint times;
! 0.75,0.75,4.5,0.2,0.2,0.4,5.0; ! Blue joint times;
0.75,0.75,4.5,2.0,0.2,0.4,5.0; ! Blue joint times;
INTERNAL SAFE OWN REAL ARRAY TIMFAC[0:1,1:7]; ! this is used by TCALC;
PRELOAD_WITH 29.5,8.375,16.24,6.05,36.6025,9.38,-62.5,-17.5,0.0,0.4,1.0, ! Yellow;
29.53125,50.805,20.24,-6.05,36.6025,10.28125,50.0,-53.0,0.0,0.4,1.0; ! Blue;
SAFE OWN REAL ARRAY ARM_CONST[0:1,1:11]; ! Arm constants;
! mnemonics for indexing into arm constants;
DEFINE BASEX = "1",
BASEY = "2",
S1 = "3",
S2 = "4",
S22 = "5",
S6 = "6",
MID1 = "7",
MID4 = "8",
MID6 = "9",
MIN_HAND = "10",
MIN_BOOM = "11";
REQUIRE "MOVE.DEF[PNT,HE]" SOURCE_FILE;
DEFINE JT1 = "'1", JT2 = "'2", JT3 = "'4", JT4 = "'10",
JT5 = "'20", JT6 = "'40", JT7 = "'100", NZ = "'200", BAD_DATA = "'400";
INTERNAL INTEGER PROCEDURE ARMSOL(INTEGER ARM; REAL ARRAY ANGLE;
RPTR(FRAME)T);
BEGIN ! for details of arm solution see Bruce's paper in 3rd progress report;
REAL TX,TY,TZ,SQRTXY,STH1,CTH1,STH2,CTH2,STH4,CTH4,STH6,CTH6,DIFF,ANG;
INTEGER FLAG,LOJOINT;
SAFE REAL ARRAY THETA[1:6];
INTEGER I,J;
REAL ARRAY TRANS[1:5,1:4];
DEFINE DEG="(180/π)";
IF (ARM=YELLOW ∧ T=F_YPARK) OR (ARM=BLUE ∧ T=F_BPARK) THEN
BEGIN "park position is always the same"
ARRBLT(ANGLE[1],PARK[ARM,1],6);
ARRBLT(THETA[1],ANGLE[1],6); ! inserted 7-14-78;
RETURN(0)
END;
FLAG←0;
ARRTRAN(TRANS,FRAME:XF[T]);
FOR I←1 STEP 1 UNTIL 3 DO
FOR J←1 STEP 1 UNTIL 3 DO
TRANS[I,J]←FRAME:XF[T][J,I];
! Solve for first three joints;
! Solve for end of boom;
TX ← TRANS[1,4] - TRANS[3,1]*ARM_CONST[ARM,S6] - ARM_CONST[ARM,BASEX];
TY ← TRANS[2,4] - TRANS[3,2]*ARM_CONST[ARM,S6] - ARM_CONST[ARM,BASEY];
TZ ← TRANS[3,4]; ! Test that we're not trying to reach through the table;
IF TZ<ARM_CONST[ARM,MIN_HAND] THEN BEGIN FLAG←NZ; TZ←ARM_CONST[ARM,MIN_HAND] END;
TZ ← TZ - TRANS[3,3]*ARM_CONST[ARM,S6];
IF TZ<ARM_CONST[ARM,MIN_BOOM] THEN BEGIN FLAG←NZ; TZ←ARM_CONST[ARM,MIN_BOOM] END;
IF (SQRTXY←TX↑2+TY↑2-ARM_CONST[ARM,S22])<0 THEN
BEGIN FLAG ← FLAG LOR JT1; SQRTXY←0 END ELSE SQRTXY←-SQRT(SQRTXY);
! Solve joint 1;
THETA[1] ← IF ABS(TY+ARM_CONST[ARM,S2])>0.001 THEN
IF TY+ARM_CONST[ARM,S2]>0 THEN 2*ATAN2(-TX+SQRTXY,TY+ARM_CONST[ARM,S2])*DEG
ELSE 2*ATAN2(TX-SQRTXY,-TY-ARM_CONST[ARM,S2])*DEG
ELSE IF TX<0 THEN 2*ATAN2(-TY,-TX)*DEG ELSE 180.0;
IF THETA[1]>HISTOP[ARM,1] THEN THETA[1]←THETA[1]-360;
IF THETA[1]<LOSTOP[ARM,1] THEN THETA[1]←THETA[1]+360;
IF THETA[1]>HISTOP[ARM,1] THEN ! Outside joint limits;
BEGIN
FLAG ← FLAG LOR JT1;
THETA[1] ← IF ANGLE[1] > ARM_CONST[ARM,MID1] THEN HISTOP[ARM,1]
ELSE LOSTOP[ARM,1]
END;
STH1 ← SIND(THETA[1]);
CTH1 ← COSD(THETA[1]);
! Solve for joint 2;
THETA[2] ← ATAN2(SQRTXY,TZ-ARM_CONST[ARM,S1])*DEG;
IF THETA[2] < LOSTOP[ARM,2] THEN
BEGIN FLAG←FLAG LOR JT2; THETA[2] ← LOSTOP[ARM,2] END
ELSE IF THETA[2] > HISTOP[ARM,2] THEN
BEGIN FLAG←FLAG LOR JT2; THETA[2] ← HISTOP[ARM,2] END; ! Check joint limits;
STH2 ← SIND(THETA[2]);
CTH2 ← COSD(THETA[2]);
! Solve for joint 3;
THETA[3] ← (TX*CTH1 +TY*STH1)/STH2;
IF THETA[3] < LOSTOP[ARM,3] THEN
BEGIN FLAG←FLAG LOR JT3; THETA[3] ← LOSTOP[ARM,3] END
ELSE IF THETA[3] > HISTOP[ARM,3] THEN
BEGIN FLAG←FLAG LOR JT3; THETA[3] ← HISTOP[ARM,3] END; ! Check joint limits;
! Solve for last three joints;
STH4 ← CTH2*(CTH1*TRANS[3,1]+STH1*TRANS[3,2])-STH2*TRANS[3,3];
CTH4 ← STH1*TRANS[3,1] - CTH1*TRANS[3,2];
! Solve for joint 5;
THETA[5] ← ABS(ATAN2(SQRT(STH4↑2+CTH4↑2),
STH2*(CTH1*TRANS[3,1]+STH1*TRANS[3,2])+CTH2*TRANS[3,3])*DEG);
IF ANGLE[5]<0 THEN THETA[5]←-THETA[5]; ! Use same side as old position;
IF THETA[5] < LOSTOP[ARM,5] THEN
BEGIN FLAG←FLAG LOR JT5; THETA[5] ← LOSTOP[ARM,5] END
ELSE IF THETA[5] > HISTOP[ARM,5] THEN
BEGIN FLAG←FLAG LOR JT5; THETA[5] ← HISTOP[ARM,5] END; ! Check joint limits;
IF ABS(THETA[5])<0.1 THEN
BEGIN "degenerate case"
THETA[4]←ANGLE[4];
THETA[6]←(ANG←ATAN2(TRANS[1,3],TRANS[2,3])*DEG) - THETA[4];
IF THETA[6]>HISTOP[ARM,6] THEN THETA[6]←THETA[6]-360;
IF THETA[6]<LOSTOP[ARM,6] THEN THETA[6]←THETA[6]+360;
IF THETA[6]>HISTOP[ARM,6] THEN ! Outside joint limits;
BEGIN ! Have to move joint 4 too;
DIFF←THETA[6];
THETA[6]←IF (THETA[6]-180)>ARM_CONST[ARM,MID6] THEN HISTOP[ARM,6]
ELSE LOSTOP[ARM,6];
DIFF←DIFF-THETA[6];
THETA[4]←THETA[4]+DIFF;
IF THETA[4]>HISTOP[ARM,4] THEN THETA[4]←THETA[4]-360;
IF THETA[4]<LOSTOP[ARM,4] THEN THETA[4]←THETA[4]+360;
IF THETA[4]>HISTOP[ARM,4] THEN ! Outside joint limits;
BEGIN ! Try flipping joint 4 by 180;
THETA[4]←IF ANGLE[4]>ARM_CONST[ARM,MID4] THEN
ANGLE[4]-180.0 ELSE ANGLE[4]-180;
IF THETA[4] < LOSTOP[ARM,4] THEN THETA[4] ← LOSTOP[ARM,4]
ELSE IF THETA[4] > HISTOP[ARM,4] THEN THETA[5] ← HISTOP[ARM,5];
! Check joint limits;
THETA[6] ← ANG - THETA[4];
IF THETA[6]>HISTOP[ARM,6] THEN THETA[6]←THETA[6]-360
ELSE IF THETA[6]<LOSTOP[ARM,6] THEN THETA[6]←THETA[6]+360;
END
END
END "degenerate case"
ELSE
BEGIN "normal case"
! Solve for joint 4;
THETA[4] ← IF THETA[5]>0 THEN ATAN2(STH4,CTH4)*DEG ELSE ATAN2(-STH4,-CTH4)*DEG;
IF (DIFF←THETA[4]-ANGLE[4])>180 THEN
BEGIN THETA[4]←THETA[4]-360; DIFF←DIFF-360 END
ELSE IF DIFF<-180 THEN BEGIN THETA[4]←THETA[4]+360; DIFF←DIFF+360 END;
IFC FALSE THENC
! NOFLIP
IF DIFF>90 THEN BEGIN THETA[4]←THETA[4]-180; THETA[5]←-THETA[5] END
ELSE IF DIFF<-90 THEN BEGIN THETA[4]←THETA[4]+180; THETA[5]←-THETA[5] END;
ENDC
IF THETA[4]<LOSTOP[ARM,4] THEN
BEGIN THETA[4]←THETA[4]+180; THETA[5]←-THETA[5] END
ELSE IF THETA[4]>HISTOP[ARM,4] THEN
BEGIN THETA[4]←THETA[4]-180; THETA[5]←-THETA[5] END;
! Solve for joint 6;
STH6←STH2*(CTH1*TRANS[2,1]+STH1*TRANS[2,2])+CTH2*TRANS[2,3];
CTH6←-STH2*(CTH1*TRANS[1,1]+STH1*TRANS[1,2])-CTH2*TRANS[1,3];
THETA[6] ← IF THETA[5]>0 THEN ATAN2(STH6,CTH6)*DEG ELSE ATAN2(-STH6,-CTH6)*DEG;
IF THETA[6]<LOSTOP[ARM,6] THEN THETA[6]←THETA[6]+360;
IF THETA[6]>HISTOP[ARM,6] THEN THETA[6]←THETA[6]-360;
IF THETA[6]<LOSTOP[ARM,6] THEN ! Try alternate solution;
IF LOSTOP[ARM,4]≤(ANG←IF THETA[4]>ARM_CONST[ARM,MID4] THEN
THETA[4]-180.0 ELSE THETA[4]+180.0)≤HISTOP[ARM,4] THEN
BEGIN "flip wrist"
THETA[6] ← THETA[6] + 180.0;
THETA[5] ← -THETA[5];
THETA[4] ← ANG
END
ELSE
BEGIN "no solution"
FLAG ← FLAG LOR JT6;
THETA[6]←IF (THETA[6]+180.0)>ARM_CONST[ARM,MID6] THEN HISTOP[ARM,6]
ELSE LOSTOP[ARM,6];
END
END "normal case";
ARRBLT(ANGLE[1],THETA[1],6);
RETURN(FLAG)
END;
END "ARMSOL"